#include <motion_control/controller/controller_p2p.h>


// 构造函数
ControllerP2P::ControllerP2P(boost::shared_ptr<PathPlanningBase> path_planner_ptr,
            boost::shared_ptr<VelocityPlanningBase> velocity_planner_ptr):
    globalPathPlannerPtr_(path_planner_ptr), velocityPlannerPtr_(velocity_planner_ptr){
    __rotate_start_direction = false;
    ResetIdle();
}

// 析构函数
ControllerP2P::~ControllerP2P(){

}

// 设置目标点数据
void ControllerP2P::SetGoal(geometry_msgs::PoseStamped pose_stamp){
    LOG(WARNING) << "set goal pose!";
    goalPose2D_ = msg_convert::ToPose2D(pose_stamp.pose);
    setGoalPose_ = true;
}

// 空闲状态
void ControllerP2P::ResetIdle(){
    pathGlobal_.clear();
}

// 执行中预处理函数
void ControllerP2P::ExecutingEntry(){
    if(!setGoalPose_){
        LOG(WARNING) << "please set goal pose!";
        SetEvent(ControllerState::Event::EV_CANCEL);
        return;
    }
    if(!globalPathPlannerPtr_){
        LOG(WARNING) << "planner_ptr is nullptr!";
        SetEvent(ControllerState::Event::EV_CANCEL);
        return;
    }

    // 生成路径
    Pose2D start = msg_convert::ToPose2D(robotPoseStamp_.pose);
    Pose2D goal = goalPose2D_;
    bool gen_state = globalPathPlannerPtr_->PathPlanning(start, goal, pathGlobal_);
    __rotate_start_direction = false;
    if(!gen_state || pathGlobal_.empty()){
        LOG(WARNING) << "generate path failed!";
        SetEvent(ControllerState::Event::EV_CANCEL);
        return;    
    }
    cout << " global_path front: " << pathGlobal_.front().transpose() << 
        ", back: " << pathGlobal_.back().transpose() << endl;
}

// 执行中处理函数
void ControllerP2P::ExecutingRunning(){
    try{
        Velocity2D vel_2d;

        if(pathGlobal_.empty()){
            throw std::runtime_error("pathGlobal_ is empty!");
        }      

        Pose2D robotPose2dGlobal = msg_convert::ToPose2D(robotPoseStamp_.pose);
        // 判断是否到达终点
        if(velocityPlannerPtr_->IsPositionReached(robotPose2dGlobal, pathGlobal_.back(), 0.2)){
            ROS_INFO("The robot reaches the goal point.");
            velocityPlannerPtr_->PublishEmptyTwist();
            SetEvent(ControllerState::Event::EV_OVER);
            return;
        }

        // 全局路径转局部路径 
        pathLocal_ = math_utils::PathGlobal2Local(robotPose2dGlobal, pathGlobal_);
        cout << "[ExecutingRunning] pathLocal_ size: " << pathLocal_.size() << endl;

        // 获取前瞻目标点
        Pose2D goalPose2dLocal, goalPose2dGlobal;
        if(!velocityPlannerPtr_->FindLookAheadPointByDistance(pathLocal_, robotPose2dGlobal, goalPose2dLocal, 0.6)){
            throw std::runtime_error("can not get target point!");
        }
        goalPose2dGlobal = math_utils::Pose2DLocal2Global(robotPose2dGlobal, goalPose2dLocal);
        cout << "[ExecutingRunning] robotPose2dGlobal: " << msg_convert::Pose2D2Str(robotPose2dGlobal) 
            << ", goalPose2dLocal: " << msg_convert::Pose2D2Str(goalPose2dLocal) 
            << ", goalPose2dGlobal: " << msg_convert::Pose2D2Str(goalPose2dGlobal) << endl;


        // 起点角度旋转, 减少不必要转弯次数
        if(!__rotate_start_direction){
            cout << "__rotate_start_direction: " << __rotate_start_direction << endl;
            if(!velocityPlannerPtr_->IsAngleReached(robotPose2dGlobal, goalPose2dGlobal, 0.2)){
                velocityPlannerPtr_->RotateToGoal(vel_2d, robotPose2dGlobal, goalPose2dGlobal);
                velocityPlannerPtr_->PublishTwist(vel_2d);
                cout << "vel_2d: " << msg_convert::Velocity2D2Str(vel_2d) << endl;
            }
            else{
                __rotate_start_direction = true;
                velocityPlannerPtr_->PublishEmptyTwist();
            }
            return;
        }

        // 计算最佳速度指令
        if(!velocityPlannerPtr_->ComputeTwist(vel_2d, vel_2d, goalPose2dLocal)){
            throw std::runtime_error("can not get vel_2d!");
        }

        // 速度指令控制
        velocityPlannerPtr_->PublishTwist(vel_2d);
    }
    catch (const std::runtime_error& e) {
        std::cerr << "[ControllerP2P | ExecutingRunning] error: " << e.what() << std::endl;
    }

}

void ControllerP2P::ExecutingExit(){
    setGoalPose_ = false;
    velocityPlannerPtr_->PublishEmptyTwist();
}